#include "TargetNearestTankState.h"
#include "Taunts.h"
#include "World.h"
#include "470bot.h"
#include "Vector2.h"
#include "KalmanTank.h"
#include "TargetingHelper.h"
#include "TimeHelper.h"
#include "RecoverFlagState.h"
#include "StrikeForceAlphaState.h"
#include "StrikeForceBetaState.h"
#include "RetrieveFallenFlag.h"

int TargetNearestTankState::tanks_sent_on_recon_mission = 0;
bool TargetNearestTankState::transition_to_moving = false;

TargetNearestTankState::TargetNearestTankState(bool move_to_tank)
{
	seekerSent = false;
	_move_to_tank = move_to_tank;
	time_of_last_seen_tank = TimeHelper::GetTime();
}


TargetNearestTankState::~TargetNearestTankState(void)
{
}

Command TargetNearestTankState::GetCommand(Agent* agent) {
	World* world = World::instance();
	tank_t* tank = agent->getTank();
	Vector2 tank_pos(tank->pos[0], tank->pos[1]);

	double min_dist = 99999999;
	KalmanTank* target = 0;

	bool targetIsBlocked = true;

	for (unsigned int i = 0; i < world->enemies().size(); i++) {
		KalmanTank* ktank = &world->enemies().at(i);
		otank_t* otank = ktank->GetStruct();
		if (otank->status != "alive")
			continue;

		double dist = (ktank->GetCurrentEstimate() - tank_pos).magnitude();
		bool isBlocked = World::instance()->blocked(tank_pos, ktank->GetCurrentEstimate());

		if(!targetIsBlocked && isBlocked) continue;

		/*if (dist < min_dist) {
			min_dist = dist;
			target = ktank;
		}*/

		if(target == 0 || (targetIsBlocked && !isBlocked)){
			min_dist = dist;
			target = ktank;
			targetIsBlocked = isBlocked;
		}
		else if(min_dist > dist){
			min_dist = dist;
			target = ktank;
			targetIsBlocked = isBlocked;
		}
	}

	if (target) {

		Vector2 target_pos = target->GetCurrentEstimate();
		if ((target_pos - tank_pos).magnitude() <= World::instance()->getShotRange() &&
			!World::instance()->intersectsAnObstacle(tank_pos, target_pos)) {
				time_of_last_seen_tank = TimeHelper::GetTime();
		}
		else if (!_move_to_tank) {
			
			// Wait 15 seconds before beginning recon
			long wait_threshold_recon = 15000;
			// Wait 20 seconds to see tanks before actively seeking other tanks out.
			long wait_threshold = 20000;
		

			
			// If we've waited long enough to see other tanks start our recon mission
			if (TimeHelper::TimeSince(time_of_last_seen_tank) > wait_threshold_recon) {
				if (tanks_sent_on_recon_mission < 4) {
					if (tanks_sent_on_recon_mission < 2) {
						agent->setState(new StrikeForceAlphaState());
					}
					else {
						agent->setState(new StrikeForceBetaState());
					}
					tanks_sent_on_recon_mission++;
					return agent->GetCommand();
				}
				/*else if(!seekerSent){
					agent->setState(new RetrieveFallenFlag());
					seekerSent = true;
					return agent->GetCommand();
				}*/
			}

			// If we've waited long enough to see other tanks, lets go find them
			// ourselves.
			if (TimeHelper::TimeSince(time_of_last_seen_tank) > wait_threshold)
				transition_to_moving = true;
			if (transition_to_moving) {
				Taunts::queue.push("I'm tired of waiting. Here I come!");

				agent->setState(new TargetNearestTankState(true));
			}
		}

		Command cmd = TargetingHelper::Target(tank, target);
		
		if (!_move_to_tank) {
			cmd.speed(0);
		}
		else {
			cmd.speed(1);
		}
		
		return cmd;
	}
	else {
		Taunts::queue.push("Ha ha ha your flag is all mine!");
		agent->setState(new RecoverFlagState(world->other_tanks()[0].color));
	}

	return Command(agent->getTank()->index, 0, 0, 0);
}